/** @file blimp.c
 * @brief Main crontrol code for the blimp.  Sets motor duty based on input from base station.
 *
 * This file contains the main control loop for the blimp.  It checks for input from the
 * base station (via the radio) and controls the motors accordingly. Based on the readings from
 * a sonar module and the desired height input from the base station it uses a PID controller to
 * maintain a constant height.  The code has the ability to output debugging info to a UART if
 * one is avaialbe, otherwise it sends limited information back to the base station for output.
 *
 *  NB!: Must be compiled with optimisation at least -O2
 *
 *    @author Aaron Kaspar
 *    @author Colburn Holbrook
 *	  @author Josh Fairhead
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "../uart.h"
#include "motors.h"
#include <string.h>
#include <stdio.h>
#include "sonar.h"
#include "height_pid.h"
#include "radio.h"
#include "../common.h"
#include <stdlib.h>


#define LED_D5_RED        0x80
#define MAX_HEIGHT		200.0//inches


/**	Max number of ranges to average. */
#define MAX_RECENT_RANGE	5

/** Max difference between ranges that will be accepted. */
#define MAX_DIFF_RANGE		20

unsigned char X=0, Y=0, Z=0;

/** The buffer to hold the packet contents. */
volatile uint8_t radio_buf[PAYLOAD_BYTES];
/* Length of buffer holding radio packet. */
volatile uint8_t packet_available;


/**
 * @brief Initializes all required components and enables interrupts.
 *
 */
static void init()
{
   //set Clock on and enable all interrupts
   CLKPR = 0x80;
   CLKPR = 0x00;


	uart_init();
	pwm_motors_init();
	sonar_init();
	height_pid_init();

	//int result = radio_init(114, 0xFEED, 1);

   sei();

   //	char buff[50];
	//sprintf(buff, "Radio init result:%d", result);
	//uart_write((uint8_t*)buff, strlen(buff));
}

/**
 * @brief Delays for count*10ms
 * 
 * This function is used to easily provide delays longer than
 * the max provided by _dealy_ms
 */
static void delay_10(int count){

	int i;
	for(i=0; i<count;i++){
		_delay_ms(10);
	}

}

/**
 * @brief Contains the control loop for the blimp
 * 
 * Begins by initializing everything and connecting to the base
 * station through the radio, then enters the control loop.
 */
int main( int argc, char *argv[] )
{
	init();


	uart_write((uint8_t *)"\n\r--------- Welcome to blimp heaven ---------\n\r", 39);

	pwm_set_duty(LEFT_MOTOR,0);
	pwm_set_duty(RIGHT_MOTOR,0);
	pwm_set_duty(CENTER_MOTOR,0);

	unsigned char prev_X=0, prev_Y=0, prev_Z=0;

	int TurnLDuty = 0;
	int TurnRDuty = 0;
 	int Thrust = 0;
	int Height = 48;
	int pid;

	int RDuty = 0;
	int LDuty = 0;
	int CDuty = 0;


	int recieved = 0;

	int i;
	uint8_t height =0;

	int busy = 0;
	char buffer [50];

	// Init an LED on the key
    PORTD ^= LED_D5_RED;
    DDRD |= LED_D5_RED;

	setheight(Height);//set default height to 4 feet

	radio_init((uint8_t)RADIO_CHANNEL, BLIMP_ADDRESS, RX_MODE);
   	packet_available = 0;


   /* Estabmish connection to base station */
   int j;
   packet p, *q;		// p is sending, *q is receiving

   p.type = READY;
   strncpy(p.payload.msg, "Blimp Connected", 16);//sizeof(p.payload.msg));

	//try three times incase packet is lost.
   for (i=0; i<3; i++)
   {
   		sprintf(buffer, "\n\r Sending READY MSG %d...", i);
		uart_write((uint8_t*)buffer, strlen(buffer));

	  radio_send(BASE_ADDRESS, (uint8_t*) &p);

   		sprintf(buffer, "Done.\n\r");
		uart_write((uint8_t*)buffer, strlen(buffer));
      for (j=0; j<4; j++)
	  {
         _delay_ms(25);
      }
   }


	radio_set_receive();

while(1){

   	if (packet_available){

		q = ((packet*) radio_buf);		// q points to radio_buf
      switch (q->type)
      {
         case CONTROL:

			X = q->payload.control.X;
			Y = q->payload.control.Y;
			Z = q->payload.control.Z;

            p.payload.status.currHeight = getHeight();
			p.payload.status.setHeight = Height;
			if(getHeight() > Height)
				p.payload.status.heightDiff = getHeight() - Height;
			else
				p.payload.status.heightDiff = Height - getHeight();

			p.type=STATUS;
            radio_send(BASE_ADDRESS, (uint8_t*) &p);

            break;
         case DEBUG:
         case STATUS:
         case READY:
         default:
            p.type = READY;
            strncpy(p.payload.msg, "Blimp Connected", 16);
            radio_send(BASE_ADDRESS, (uint8_t*) &p);
            break;
      }
         packet_available = 0;

		 radio_set_receive();

	}

//***************************************************************
//Here is the turn control
//***************************************************************
		height = getHeight();


		sprintf(buffer, "BASE: X=%d, Y=%d, Z=%d. BLIMP: currHeight=%d\n\r", X, Y, Z, getHeight());
		uart_write((uint8_t*)buffer, strlen(buffer));


		if(prev_X != X){

			if (X < 120){

				TurnRDuty = (255.0/120)*X - 255;
				TurnLDuty = -(255.0/120)*X + 255;

			} else if (X > 130){

				TurnRDuty = (255.0/125)*X - 265;
				TurnLDuty = -(255.0/125)*X + 265;

			}

			else {

				TurnRDuty = 0;
				TurnLDuty = 0;

			}

			prev_X = X;

		}


//***************************************************************
//Here is the thrust control
//***************************************************************

	if(prev_Y != Y){

		if (Y < 130){

			Thrust = (255.0/130)*Y - 255;

		} else if (Y > 160){

			Thrust = (255.0/95)*Y - 429;

		}

		else {

			Thrust = 0;

		}

		prev_Y = Y;

	}

//***************************************************************
//Here is the height control
//***************************************************************

	if (prev_Z != Z){

		Height = (MAX_HEIGHT/255)*Z - 2;

		setheight(Height);

		prev_Z = Z;

	}

	pid = height_pid();

//***************************************************************
//Serial Output
//***************************************************************

	//	sprintf (buffer, "Height:%d CDuty:%d PID:%d\n\r", Height, CDuty, pid);
	//	uart_write((uint8_t*)buffer, strlen(buffer));

		delay_10(3);



//***************************************************************
//Here is the motor control
//***************************************************************

		RDuty = Thrust + TurnRDuty;
		LDuty = Thrust + TurnLDuty;
		CDuty = height_scale(pid);

		if (RDuty >= 255)
			RDuty = 254;

		if (LDuty >= 255)
			LDuty = 254;

		if (RDuty <= -255)
			RDuty = -254;

		if (LDuty <= -255)
			LDuty = -254;

		pwm_set_duty(RIGHT_MOTOR, RDuty);
		pwm_set_duty(LEFT_MOTOR, LDuty);
		pwm_set_duty(CENTER_MOTOR, CDuty);

    //***************************************************************
	//Here is the sonar control
	//***************************************************************

		if (busy == 0){
			trigger_sonar();
			busy = 1;

			}

		recieved = signal_recieved();

		delay_10(3);

		if (recieved ==1){
			busy = 0;
		}

	}

	return 0;
}


